#include "rotatedgaussseidelsolver.h"
#include <cmath>
using namespace std;
#define PI 3.1415926
#define DEGTORAD(Deg) ((Deg * PI) / 180.0)
#define RADTODEG(Rad) ((180.0 * Rad) / PI)
RotatedGaussSeidelSolver::RotatedGaussSeidelSolver(int _n, double mu1, double e1, QObject * parent):
        GaussSeidelSolver(_n,mu1, e1, parent), alpha(DEGTORAD(30))//, ro(0)
{

}

double RotatedGaussSeidelSolver::iteration()
{
    double maxdx = 0;
    for (int i = 0; i < k; i++)
    {
        double x_new = 0.0;
        double y_new = 0.0;
        double newdx = 0.0;
        Zone  b;
        if (coeffs_map[i] == -1)
            continue;
        b = coeffs_list[coeffs_map[i]];
//        for (int ii = 0; ii < 2; ii++)
//        {
//            for (int j = 0; j < 2; j++)
//            {
//                for (int p = 0; p < 7; p++)
//                    b[ii][j][p] = coeffs_list[coeffs_map[i]][ii][j][p];
//            }
//        }

        int column = i % m;
        int row = i / m;

        // top border, fixed
        if (row == 1)
        {
            x[i] = 0;
            x[i+k] = 0;
            continue ;
        }
        // central points
        else if ((row + column) < (2 *n - 1))
        {
            double sum = 0;
            double gama = 0;
            //x
            sum =                        x[i+1]* b[0][0][1] +   x[i-m+1]* b[0][0][2] +   x[i-m]* b[0][0][3]+   x[i-1]* b[0][0][4] +   x[i+m-1]* b[0][0][5] +   x[i+m]* b[0][0][6] +
                  + x[i+k]* b[0][1][0] + x[i+1+k]* b[0][1][1] + x[i-m+1+k]* b[0][1][2] + x[i-m+k]* b[0][1][3] + x[i-1+k]* b[0][1][4] + x[i+m-1+k]* b[0][1][5] + x[i+m+k]* b[0][1][6];
            if (column == 1)
            {
                if (row == 1)
                    gama = sum - f1*h/2;
                    //gama = 0.0;
                else if (row < n)
                    gama = sum - f1*h;
                else if (row == n)
                    gama = sum - f1*(h+h/t)/2.0;
                else //if (row < 2*n-1)
                    gama = sum - f1*h/t;
                //else
                //    gama = sum - f1*h/(2*t);
            }
            else if (column == n)
            {
                if (row == 1)
                    gama = sum - f2*h/2;
                else //if (row < n)
                    gama = sum - f2*h;
                //else
                //    gama = sum - f2*h/2;
            }
            else
            {
                gama = sum;
            }
            x_new = -(gama) / b[0][0][0];

            //y
            sum = x[i]*b[1][0][0] + x[i+1]*b[1][0][1] +   x[i-m+1]*b[1][0][2] +   x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +   x[i+m-1]*b[1][0][5] +   x[i+m]*b[1][0][6] +
                                  + x[i+1+k]*b[1][1][1] + x[i-m+1+k]*b[1][1][2] + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4] + x[i+m-1+k]*b[1][1][5] + x[i+m+k]*b[1][1][6];
            gama = sum;
            y_new = -(gama) / b[1][1][0];

        }
        // points near to bottom border
        else if ((row + column) == (2 * n - 1))
        {
            double sum = 0;
            double gama = 0;
            // x
            sum =                       x[i-m+1]*b[0][0][2] +   x[i-m]*b[0][0][3] +   x[i-1]*b[0][0][4] +   x[i+m-1]*b[0][0][5] +
                  + x[i+k]*b[0][1][0] + x[i-m+1+k]*b[0][1][2] + x[i-m+k]*b[0][1][3] + x[i-1+k]*b[0][1][4] + x[i+m-1+k]*b[0][1][5];
            sum +=   b[0][0][1] * (cos(alpha)*x[i+1] - sin(alpha)*x[i+1+k]) + b[0][0][6] * (cos(alpha)*x[i+m] - sin(alpha)*x[i+m+k]) +
                   + b[0][1][1] * (sin(alpha)*x[i+1] + cos(alpha)*x[i+1+k]) + b[0][1][6] * (sin(alpha)*x[i+m] + cos(alpha)*x[i+m+k]);
            if (column == 1)
            {
                //gama = sum - f1*h/(2*t);
                gama = sum - f1*h/(t);
            }
            else if (column == n)
                gama = sum - f2*h;
            else
                gama = sum;
            x_new = -(gama) / b[0][0][0];

            // y
            sum = x[i]*b[1][0][0] + x[i-m+1]*b[1][0][2] +   x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +   x[i+m-1]*b[1][0][5] +
                                  + x[i-m+1+k]*b[1][1][2] + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4] + x[i+m-1+k]*b[1][1][5];
            sum +=   b[1][0][1] * (cos(alpha)*x[i+1] - sin(alpha)*x[i+1+k]) + b[1][0][6] * (cos(alpha)*x[i+m] - sin(alpha)*x[i+m+k]) +
                   + b[1][1][1] * (sin(alpha)*x[i+1] + cos(alpha)*x[i+1+k]) + b[1][1][6] * (sin(alpha)*x[i+m] + cos(alpha)*x[i+m+k]);
            gama = sum;
            y_new = -(gama) / b[1][1][0];
        }
        // points on the border
        else if ((row + column) == (2 * n))
        {
            double sum = 0.0;
            double gama = 0.0;
            // x
//            double sum = x[i+k] * (-cos(alpha)*sin(alpha)*b[0][0][0] + cos(2*alpha)*b[0][1][0]);
//            sum +=   x[i-m]*b[0][0][3]*cos(alpha)   + x[i-1]*b[0][0][4]*cos(alpha) +
//                   + x[i-m+k]*b[0][1][3]*cos(alpha) + x[i-1+k]*b[0][1][4]*cos(alpha);
//            sum +=   b[0][0][2]*cos(alpha) * (x[i-m+1]*cos(alpha) - x[i-m+1+k]*sin(alpha)) +
//                   + b[0][0][5]*cos(alpha) * (x[i+m-1]*cos(alpha) - x[i+m-1+k]*sin(alpha)) +
//                   + b[0][1][2]*cos(alpha) * (x[i-m+1]*sin(alpha) + x[i-m+1+k]*cos(alpha)) +
//                   + b[0][1][5]*cos(alpha) * (x[i+m-1]*sin(alpha) + x[i+m-1+k]*cos(alpha));
            sum =   x[i-m]*b[0][0][3] +   x[i-1]*b[0][0][4] +
                  + x[i-m+k]*b[0][1][3] + x[i-1+k]*b[0][1][4];
            sum +=   b[0][0][2] * (x[i-m+1]*cos(alpha) - x[i-m+1+k]*sin(alpha)) + b[0][0][5] * (x[i+m-1]*cos(alpha) - x[i+m-1+k]*sin(alpha)) +
                   + b[0][1][2] * (x[i-m+1]*sin(alpha) + x[i-m+1+k]*cos(alpha)) + b[0][1][5] * (x[i+m-1]*sin(alpha) + x[i+m-1+k]*cos(alpha));

            sum += (b[0][0][0] * (-sin(alpha)) +  b[0][1][0] * (cos(2*alpha)/cos(alpha))) * x[i+k];
            if (column == 1)
//                gama = sum - (f1*h/(2.0*t)*cos(alpha));
                gama = sum - (f1*h/(2.0*t));
            else if (column == n)
//                gama = sum - (f2*h/2.0*cos(alpha));
                gama = sum - (f2*h/2.0);
            else
                gama = sum;
//            x_new = -(gama) / (b[0][0][0]*cos(alpha)*cos(alpha) + b[0][1][0]*sin(2*alpha));
            if (friction)
            {
                double gg = g_tr * h/cos(alpha);
                if (column == 1 || column == n)
                    gg /= 2;
                if (gama > gg)
                    gama -= gg;
                else if (gama < gg)
                    gama += gg;
                else
                    gama = 0;
            }
            double x_temp = -(gama) / (b[0][0][0]*cos(alpha) + b[0][1][0]*2*sin(alpha));



            // y
            sum =   x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +
                  + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4];
            sum += b[1][0][2] * (x[i-m+1]*cos(alpha) - x[i-m+1+k]*sin(alpha)) +
                 + b[1][0][5] * (x[i+m-1]*cos(alpha) - x[i+m-1+k]*sin(alpha)) +
                 + b[1][1][2] * (x[i-m+1]*sin(alpha) + x[i-m+1+k]*cos(alpha)) +
                 + b[1][1][5] * (x[i+m-1]*sin(alpha) + x[i+m-1+k]*cos(alpha));
            sum += (b[1][0][0] * (cos(2*alpha)/cos(alpha)) + b[1][1][0] * sin(alpha)) * x[i];
            gama = sum;
//            x_new = -(gama) / (b[1][1][0]*cos(alpha)*cos(alpha) -b[1][0][0]*sin(2*alpha));

            double y_temp = -(gama) / (b[1][0][0] * 2 * (-sin(alpha)) + b[1][1][0] * cos(alpha));

            //x_new = cos(-alpha) * x_temp - sin(-alpha) * y_temp;
           // y_new = sin(-alpha) * x_temp + cos(-alpha) * y_temp;
            x_new = x_temp;
            y_new = y_temp;

            //x_new, y_new = normal_from_rotated(x[ind], x[ind+k], alpha)
            if (unpenetration && y_new < 0)
                y_new = 0.0;
        }

        newdx = abs(x_new - x[i]);
        if (newdx > maxdx)
            maxdx = newdx;
        x[i] = x_new;
        newdx = abs(y_new - x[i+k]);
        if (newdx > maxdx)
            maxdx = newdx;
        x[i+k] = y_new;


//        next_point :
//        x_new = 0.0;
//        if (row == 1)
//        {
//            x[i+k] = 0;
//            continue;
//        }
//        else if ((row + column) < (2 *n - 1))
//        {
//            double sum = x[i]*b[1][0][0] + x[i+1]*b[1][0][1] +   x[i-m+1]*b[1][0][2] +   x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +   x[i+m-1]*b[1][0][5] +   x[i+m]*b[1][0][6] +
//                                         + x[i+1+k]*b[1][1][1] + x[i-m+1+k]*b[1][1][2] + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4] + x[i+m-1+k]*b[1][1][5] + x[i+m+k]*b[1][1][6];
//            double gama = sum;
//            x_new = -(gama) / b[1][1][0];

//        }
//        else if ((row + column) == (2 *n - 1))
//        {
//            double sum = x[i]*b[1][0][0] + x[i-m+1]*b[1][0][2] +   x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +   x[i+m-1]*b[1][0][5] +
//                                         + x[i-m+1+k]*b[1][1][2] + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4] + x[i+m-1+k]*b[1][1][5];
//            sum +=   b[1][0][1] * (cos(alpha)*x[i+1] - sin(alpha)*x[i+1+k]) + b[1][0][6] * (cos(alpha)*x[i+m] - sin(alpha)*x[i+m+k]) +
//                   + b[1][1][1] * (sin(alpha)*x[i+1] + cos(alpha)*x[i+1+k]) + b[1][1][6] * (sin(alpha)*x[i+m] + cos(alpha)*x[i+m+k]);
//            double gama = sum;
//            x_new = -(gama) / b[1][1][0];
//        }
//        else if ((row + column) == (2 * n))
//        {
////            double sum =   x[i]*(sin(alpha)*cos(alpha)*b[1][1][0]+cos(2*alpha)*b[1][0][0]) +
////                         + x[i-m]*b[1][0][3]*cos(alpha) +   x[i-1]*b[1][0][4]*cos(alpha) +
////                         + x[i-m+k]*b[1][1][3]*cos(alpha) + x[i-1+k]*b[1][1][4]*cos(alpha);
////            sum +=   b[1][0][2]*cos(alpha) * (x[i-m+1]*cos(alpha) - x[i-m+1+k]*sin(alpha)) +
////                   + b[1][0][5]*cos(alpha) * (x[i+m-1]*cos(alpha) - x[i+m-1+k]*sin(alpha)) +
////                   + b[1][1][2]*cos(alpha) * (x[i-m+1]*sin(alpha) + x[i-m+1+k]*cos(alpha)) +
////                   + b[1][1][5]*cos(alpha) * (x[i+m-1]*sin(alpha) + x[i+m-1+k]*cos(alpha));
//            double sum = x[i-m]*b[1][0][3] +   x[i-1]*b[1][0][4] +
//                       + x[i-m+k]*b[1][1][3] + x[i-1+k]*b[1][1][4];
//            sum += b[1][0][2] * (x[i-m+1]*cos(alpha) - x[i-m+1+k]*sin(alpha)) +
//                 + b[1][0][5] * (x[i+m-1]*cos(alpha) - x[i+m-1+k]*sin(alpha)) +
//                 + b[1][1][2] * (x[i-m+1]*sin(alpha) + x[i-m+1+k]*cos(alpha)) +
//                 + b[1][1][5] * (x[i+m-1]*sin(alpha) + x[i+m-1+k]*cos(alpha));
//            sum += (b[1][0][0] * (cos(alpha) - sin(alpha)*tan(alpha)) + b[1][1][0] * sin(alpha)) * x[i];
//            double gama = sum;
////            x_new = -(gama) / (b[1][1][0]*cos(alpha)*cos(alpha) -b[1][0][0]*sin(2*alpha));
//            x_new = -(gama) / (b[1][0][0] * 2 * (-sin(alpha)) + b[1][1][0] * cos(alpha));
//            if (unpenetration && x_new < 0)
//                x_new = 0;

//        newdx = abs(x_new - x[i+k]);
//        if (newdx > maxdx)
//            maxdx = newdx;
//        x[i+k] = x_new;
    }
    return maxdx;
}

//int RotatedGaussSeidelSolver::solve()
//{
//    int temp = GaussSeidelSolver::solve();
//    for (int i =1; i < m-1; i++)
//    {
//        int ind = 2*m*n-i*(m-1);
//        double x_new = cos(alpha) * x[ind] - sin(alpha) * x[ind+k];
//        double y_new = sin(alpha) * x[ind] + cos(alpha) * x[ind+k];
//        //x_new, y_new = normal_from_rotated(x[ind], x[ind+k], alpha)
//        x[ind] = x_new;
//        x[ind+k] = y_new;
//        //x[ind], x[ind+k] = x_new, y_new;
//    }
//    return temp;
//}

//void RotatedGaussSeidelSolver::turnCoordinatesBack()
//{
//    for (int i =1; i < m-1; i++)
//    {
//        int ind = 2*m*n-i*(m-1);
//        double x_new = cos(alpha) * x[ind] - sin(alpha) * x[ind+k];
//        double y_new = sin(alpha) * x[ind] + cos(alpha) * x[ind+k];
//        //x_new, y_new = normal_from_rotated(x[ind], x[ind+k], alpha)
//        x[ind] = x_new;
//        x[ind+k] = y_new;
//        //x[ind], x[ind+k] = x_new, y_new;
//    }
//}

int RotatedGaussSeidelSolver::solve()
{
    int result = GaussSeidelSolver::solve();
    for (int i =1; i < m-1; i++)
    {
        int ind = 2*m*n-i*(m-1);
        double x_new = cos(alpha) * x[ind] - sin(alpha) * x[ind+k];
        double y_new = sin(alpha) * x[ind] + cos(alpha) * x[ind+k];
        //x_new, y_new = normal_from_rotated(x[ind], x[ind+k], alpha)
        x[ind] = x_new;
        x[ind+k] = y_new;
        //x[ind], x[ind+k] = x_new, y_new;
    }
    return result;
}

double RotatedGaussSeidelSolver::updateLagrange(double ro)
{
    double outer_dx = 0.0;
    for (int i = 1; i < m-1; i++)
    {
        int ind = 2*m*n-i*(m-1);
        l[i] = max(0.0, l[i] - ro * x[ind+k]);
    }
    for (int i = 0; i < m; i++)
    {
        if (abs(l[i] - lOld[i]) > outer_dx)
            outer_dx = abs(l[i] - lOld[i]);
        lOld[i] = l[i];
    }
    return outer_dx;
}

//double BaseGaussSeidelSolver::updateLagrange()
//{
//    double outer_dx = 0.0;
//    for (int i =1; i < m-1; i++)
//    {
//        int ind = 2*m*n-i*(m-1);
//        l[i] = max(0.0, l[i] - ro * x[ind+k]);
//    }
//    for (int i = 0; i < m; i++)
//    {
//        if (abs(l[i] - lOld[i]) > outer_dx)
//            outer_dx = abs(l[i] - lOld[i]);
//        lOld[i] = l[i];
//    }
//    return outer_dx;
//}
